#include <stdlib.h>
#include "avr_compiler.h"
#include "uart.h"
#include <stdio.h>
#include "pmd_control.h"
#include "pc_comm.h"
#include "util/delay.h"

void pc_init(PC_Control_t * pc, USART_t * uart, int baud, PMD_Control_t * pmd
){

	if(uart == &USARTE0){
		PORTE.DIR = 0x08;
		uartInitiate(&(pc->uart), uart, baud);
	}
	//Estop setup stuff
	pc->temp_max = DEFAULT_MAX_TEMP;
	pc->bus_voltage_upper_limit = DEFAULT_VOLTAGE_UPPER_LIMIT;
	pc->bus_voltage_lower_limit = DEFAULT_VOLTAGE_LOWER_LIMIT;


	pc->command_source = CMD_SOURCE_PROFILE_GEN;
	pc->spi_motor_command = SPI_COMMAND_DEFAULT;
	pc->status = 0;
	pc->serial_flag = 0;
	pc->bus_voltage = 0;
	pc->serial_mode = 0;
	pc->data_in_count = 0;
	pc->data_available = 0;
	pc->check_sum = 0;
	pc->motor_mode = MOTOR_MODE_DEFAULT;
	pc->commutation_mode = COMMUTATION_MODE_DEFAULT;
	pc->loop_mode = LOOP_MODE_DEFAULT;
	pc->dead_time = DEAD_TIME_DEFAULT;
	pc->motor_armed = MOTOR_ARMED_DEFAULT;
	pc->motor_command = MOTOR_COMMAND_DEFAULT;
	pc->condition_mask1 = CONDITION_MASK1_DEFAULT;
	pc->condition_mask2 = CONDITION_MASK2_DEFAULT;
	pc->phase_counts = PHASE_COUNTS_DEFAULT;

	pc->motor_limit = MOTOR_LIMIT_DEFAULT;
	pc->pwm_sense = PWM_SENSE_DEFAULT;
	pc->signal_sense = SIGNAL_SENSE_DEFAULT;

	pc->velocity_scalar = VELOCITY_SCALAR_DEFAULT;
	pc->motor_accelleration = (uint32_t)MOTOR_ACCELLERATION_DEFAULT;

	pc->velocity_loop_gain_kp = VELOCITY_LOOP_GAIN_KP_DEFAULT;
	pc->velocity_loop_gain_ki = VELOCITY_LOOP_GAIN_KI_DEFAULT;
	pc->velocity_loop_gain_limit = VELOCITY_LOOP_GAIN_LIMIT_DEFAULT;

	pc->current_loop_gain_kp = CURRENT_LOOP_GAIN_KP_DEFAULT;
	pc->current_loop_gain_ki = CURRENT_LOOP_GAIN_KI_DEFAULT;
	pc->current_loop_gain_limit = CURRENT_LOOP_GAIN_LIMIT_DEFAULT;

	pc->phase_correction_mode = PHASE_CORRECTION_MODE_DEFAULT;

	pc->motion_error_limit = MOTION_ERROR_LIMIT_DEFAULT;



	_delay_ms(2000);

	pmd_no_operation(pmd);

	set_motor_mode(pmd, pc->motor_mode);
	set_commutation_mode(pmd, pc->commutation_mode);
	set_loop_mode(pmd, pc->loop_mode);
	set_dead_time(pmd, pc->dead_time);
	set_motor_command(pmd, pc->motor_command);
	set_condition_mask(pmd, pc->condition_mask1, pc->condition_mask2);
	set_motor_limit(pmd, pc->motor_limit);
	set_pwm_sense(pmd, pc->pwm_sense);
	set_signal_sense(pmd, pc->signal_sense);
	set_phase_counts(pmd, pc->phase_counts);
	
	set_loop_gain(pmd, LOOP_VELOCITY_KP, (uint32_t)pc->velocity_loop_gain_kp);
	
	set_loop_gain(pmd, LOOP_VELOCITY_KI, (uint32_t)pc->velocity_loop_gain_ki);
	set_loop_gain(pmd, LOOP_VELOCITY_LIMIT, (uint32_t)pc->velocity_loop_gain_limit);

	set_loop_gain(pmd, LOOP_CURRENT_KP, (uint32_t)pc->current_loop_gain_kp);
	
	set_loop_gain(pmd, LOOP_CURRENT_KI, (uint32_t)pc->current_loop_gain_ki);
	set_loop_gain(pmd, LOOP_CURRENT_LIMIT, (uint32_t)pc->current_loop_gain_limit);


	set_velocity_scalar(pmd, pc->velocity_scalar);
	set_acceleration(pmd, (uint32_t)pc->motor_accelleration);
	set_command_source(pmd, pc->command_source);
	set_velocity_feedback_source(pmd, VELOCITY_FEEDBACK_SOURCE_ENCODER);

	set_phase_correction(pmd, pc->phase_correction_mode);

	set_error_limit(pmd, pc->motion_error_limit);

	pmd_no_operation(pmd);
	
}

void pc_acknowledge(PC_Control_t * pc){
	int i;
	uint8_t string[] = {pc->status, 0,
				(uint8_t)(pc->bus_voltage >> 8), 
				(uint8_t)(pc->bus_voltage & 0xff),
				(uint8_t)(pc->temp)};
	
	string[1] = generate_checksum(string, 5);

	for(i=0;i<5;i++){
		sendchar(&(pc->uart), string[i]);
	}
}

void pc_give_command(PC_Control_t * pc){
	int i;
	uint8_t string[] = {pc->status, 0,
				(uint8_t)(pc->motor_command >> 8),
				(uint8_t)(pc->motor_command & 0xff)};
	
	string[1] = generate_checksum(string, 4);

	for(i=0;i<4;i++){
		sendchar(&(pc->uart), string[i]);
	}
}
void pc_give_event_status(PC_Control_t * pc){
	int i;
	uint8_t string[] = {pc->status, 0,
				(uint8_t)(pc->event_status>> 8),
				(uint8_t)(pc->event_status & 0xff)};
	
	string[1] = generate_checksum(string, 4);

	for(i=0;i<4;i++){
		sendchar(&(pc->uart), string[i]);
	}
}
void pc_give_current(PC_Control_t * pc){
	int i;
	uint8_t string[] = {pc->status, 0,
				(uint8_t)(pc->current_A >> 8),
				(uint8_t)(pc->current_A & 0xff),
				(uint8_t)(pc->current_B >> 8),
				(uint8_t)(pc->current_B & 0xff)};
	
	string[1] = generate_checksum(string, 6);

	for(i=0;i<6;i++){
		sendchar(&(pc->uart), string[i]);
	}
}


void pc_give_velocity(PC_Control_t * pc){
	int i;
	if(pc->motor_velocity > 0){
		//PORTA.OUT ^= 3;
	}
	uint8_t string[] = {pc->status, 0,
				(uint8_t)(pc->motor_velocity >> 24),
				(uint8_t)(pc->motor_velocity >> 16),
				(uint8_t)(pc->motor_velocity >> 8),
				(uint8_t)(pc->motor_velocity & 0xff)};
	
	string[1] = generate_checksum(string, 6);

	for(i=0;i<6;i++){
		sendchar(&(pc->uart), string[i]);
	}
}

void pc_give_spi(PC_Control_t * pc){
	int i;
	uint8_t string[] = {pc->status, 0,
				(uint8_t)(pc->spi_motor_command >> 8),
				(uint8_t)(pc->spi_motor_command & 0xff)};
	
	string[1] = generate_checksum(string, 4);

	for(i=0;i<4;i++){
		sendchar(&(pc->uart), string[i]);
	}
}


void pc_give_error(PC_Control_t * pc){
	int i;
	uint8_t string[] = {pc->status, 0,
				(uint8_t)(pc->error_A>> 8),
				(uint8_t)(pc->error_A & 0xff),
				(uint8_t)(pc->error_B >> 8),
				(uint8_t)(pc->error_B & 0xff)};
	
	string[1] = generate_checksum(string, 6);

	for(i=0;i<6;i++){
		sendchar(&(pc->uart), string[i]);
	}
}


void send_pc_update(PC_Control_t * pc){
	int i;
	//21 bytes long
	uint8_t string[40];
	for(i=0;i<40;i++){
		string[i] = 0;
	}
	string[0] = 'v';
	string[1] = 'e';
	string[2] = 'x';
	string[32] = 'u';
	string[4] = pc->motor_mode;
	string[5] = pc->commutation_mode;
	string[6] = pc->loop_mode;
	string[7] = pc->dead_time;
	string[8] = (uint8_t)(pc->motor_limit >> 8);
	string[9] = (uint8_t)(pc->motor_limit & 0xff);
	string[10] = (uint8_t)(pc->phase_counts >> 8);
	string[11] = (uint8_t)(pc->phase_counts & 0xff);
	string[12] = (uint8_t)(pc->velocity_scalar >> 8);
	string[13] = (uint8_t)(pc->velocity_scalar & 0xff);
	string[14] = (uint8_t)(pc->motor_accelleration >> 24);
	string[15] = (uint8_t)(pc->motor_accelleration >> 16);
	string[16] = (uint8_t)(pc->motor_accelleration >> 8);
	string[17] = (uint8_t)(pc->motor_accelleration & 0xff);

	string[18] = (uint8_t)(pc->velocity_loop_gain_kp >> 8);
	string[19] = (uint8_t)(pc->velocity_loop_gain_kp & 0xff);
	string[20] = (uint8_t)(pc->velocity_loop_gain_ki >> 8);
	string[21] = (uint8_t)(pc->velocity_loop_gain_ki & 0xff);
	string[22] = (uint8_t)(pc->velocity_loop_gain_limit >> 8);
	string[23] = (uint8_t)(pc->velocity_loop_gain_limit & 0xff);
	string[24] = (uint8_t)(pc->current_loop_gain_kp >> 8);
	string[25] = (uint8_t)(pc->current_loop_gain_kp & 0xff);
	string[26] = (uint8_t)(pc->current_loop_gain_ki >> 8);
	string[27] = (uint8_t)(pc->current_loop_gain_ki & 0xff);
	string[28] = (uint8_t)(pc->current_loop_gain_limit >> 8);
	string[29] = (uint8_t)(pc->current_loop_gain_limit & 0xff);
	string[30] = (uint8_t)(pc->temp_max) + 55;
	string[31] = (uint8_t)(pc->command_source);



	string[3] = generate_checksum(string, 33);
	for(i=0;i<33;i++){
		sendchar(&(pc->uart), string[i]);
	}

}

void pc_set_pmd(PC_Control_t * pc, PMD_Control_t * pmd){
	
	PORTD.OUT &= ~0b00100000;
	set_command_source(pmd, pc->command_source);
	set_motor_mode(pmd, pc->motor_mode);
	set_commutation_mode(pmd, pc->commutation_mode);
	set_loop_mode(pmd, pc->loop_mode);
	set_dead_time(pmd, pc->dead_time);
	set_motor_limit(pmd, pc->motor_limit);
	set_pwm_sense(pmd, pc->pwm_sense);
	set_phase_counts(pmd, pc->phase_counts);

	set_loop_gain(pmd, LOOP_CURRENT_KP, (uint32_t)pc->current_loop_gain_kp);
	
	set_loop_gain(pmd, LOOP_CURRENT_KI, (uint32_t)pc->current_loop_gain_ki);
	set_loop_gain(pmd, LOOP_CURRENT_LIMIT, (uint32_t)pc->current_loop_gain_limit);

	set_loop_gain(pmd, LOOP_VELOCITY_KP, (uint32_t)pc->velocity_loop_gain_kp);
	
	set_loop_gain(pmd, LOOP_VELOCITY_KI, (uint32_t)pc->velocity_loop_gain_ki);
	set_loop_gain(pmd, LOOP_VELOCITY_LIMIT, (uint32_t)pc->velocity_loop_gain_limit);
	set_velocity_scalar(pmd, pc->velocity_scalar);
	set_acceleration(pmd, (uint32_t)pc->motor_accelleration);

}
void pc_handle(PC_Control_t * pc){
	USART_RXComplete(&(pc->uart));
	pc->data_in[pc->data_in_count] = USART_RXBuffer_GetByte(&(pc->uart));

	if(pc->serial_mode == 1){
		pc->data_available = 1;
	}

	else{

		if((pc->data_in[0] == 'v') && (pc->data_in_count == 0)){

			pc->check_sum = pc->data_in[0];
			pc->data_in_count ++;
			pc->data_request_flag = 0;
			pc->serial_flag = 0;

		}
		else if((pc->data_in[1] == 'e') && (pc->data_in_count == 1) && (pc->serial_flag == 0)){

			pc->check_sum += pc->data_in[1];
			pc->data_in_count ++;
			pc->data_request_flag = 0;
			

		}
		else if((pc->data_in[2] == 'x') && (pc->data_in_count == 2) && (pc->serial_flag == 0)){

			pc->check_sum += pc->data_in[2];
			pc->data_in_count ++;
			pc->data_request_flag = 0;

		}
		else if((pc->data_in_count > 2) && (pc->data_in_count < 6) && (pc->data_request_flag == 0) && (pc->serial_flag == 0)){

			pc->check_sum += pc->data_in[pc->data_in_count];
			pc->data_in_count ++;
		}

		else if((pc->data_in_count > 5) && (pc->data_in_count < pc->data_in[5]) && (pc->data_request_flag == 0) && (pc->serial_flag == 0)){
			pc->check_sum += pc->data_in[pc->data_in_count];
			pc->data_in_count ++;
		}
		else if((pc->data_in[pc->data_in[5]] == 'u') && (pc->data_in_count == pc->data_in[5]) && (pc->data_request_flag == 0) && (pc->serial_flag == 0)){
			pc->check_sum += pc->data_in[pc->data_in_count];
			pc->data_in_count ++;

			if(pc->check_sum){
				pc->data_in_count = 0;
			}
			else{
				pc->data_available = 1;
			}
		}
		else if((pc->data_in[0] == 'g') && (pc->data_in_count == 0)){
			pc->data_request_flag = 1;
			pc->data_in_count ++;



		}
		else if((pc->data_in[1] == 'i') && (pc->data_in_count == 1) && (pc->data_request_flag == 1)){
			pc->data_in_count ++;

		}
		else if((pc->data_in[2] == 'v') && (pc->data_in_count == 2) && (pc->data_request_flag == 1)){
			pc->data_in_count ++;

		}
		else if((pc->data_in[3] == 'e') && (pc->data_in_count == 3) && (pc->data_request_flag == 1)){
			pc->data_in_count ++;
			pc->data_read_request = 1;
		}
		else if((pc->data_in[pc->data_in_count] == '~')){
			pc->data_in_count = 0;

		}
		else if((pc->data_in[0] == 's') && (pc->data_in_count == 0)){
			pc->data_in_count ++;
			pc->serial_flag = 1;
		}
		else if(pc->serial_flag){
			if((pc->data_in[1] == 'e') && (pc->data_in_count == 1)){
				pc->data_in_count ++;
			}
			else if((pc->data_in[2] == 'r') && (pc->data_in_count == 2)){
				pc->data_in_count ++;
			}
			else if((pc->data_in[3] == 'i') && (pc->data_in_count == 3)){
				pc->data_in_count ++;
			}
			else if((pc->data_in[4] == 'a') && (pc->data_in_count == 4)){
				pc->data_in_count ++;
			}
			else if((pc->data_in[5] == 'l') && (pc->data_in_count == 5)){
				pc->data_in_count ++;
				pc->serial_mode = 1;
				pc->data_available = 1;
			}
		}
		else{

		}
	}
}
	


